
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_geodesic_grid.c
  * @author     baiyang
  * @date       2021-12-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_geodesic_grid.h"

#include <stdint.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
struct neighbor_umbrella {
    /**
     * The umbrella's components. The value of #components[i] is the
     * icosahedron triangle index of the i-th component.
     *
     * In order to find the components for T_10, the following just finding
     * the index of the opposite triangle is enough. In other words,
     * (#components[i] + 10) % 20.
     */
    uint8_t components[5];
    /**
     * The fields with name in the format vi_cj are interpreted as the
     * following: vi_cj is the index of the vector, in the icosahedron
     * triangle pointed by #components[j], that matches the umbrella's i-th
     * vertex.
     *
     * The values don't change for T_10.
     */
    uint8_t v0_c0;
    uint8_t v1_c1;
    uint8_t v2_c1;
    uint8_t v4_c4;
    uint8_t v0_c4;
};

/*---------------------------------prototype----------------------------------*/
static int _neighbor_umbrella_component(int idx, int comp_idx);
static int _from_neighbor_umbrella(int idx, const Vector3f_t *v, const Vector3f_t *u, bool inclusive);
static int _triangle_index(const Vector3f_t *v, bool inclusive);
static int _subtriangle_index(const unsigned int triangle_index, const Vector3f_t *v, bool inclusive);
/*----------------------------------variable----------------------------------*/
const struct neighbor_umbrella _neighbor_umbrellas[3] = {
    {{ 9,  8,  7, 12, 14}, 1, 2, 0, 0, 2},
    {{ 1,  2,  4,  5,  3}, 0, 0, 2, 2, 0},
    {{16, 15, 13, 18, 17}, 2, 2, 0, 2, 1},
};


/**
 * Number of sub-triangles for an icosahedron triangle.
 */
static const int NUM_SUBTRIANGLES = 4;

/**
 * The inverses of the change-of-basis matrices for the icosahedron
 * triangles.
 *
 * The i-th matrix is the inverse of the change-of-basis matrix from
 * natural basis to the basis formed by T_i's vectors.
 */
const static matrix3f_t _inverses[10] = {
    {{-0.309017f,  0.500000f,  0.190983f},
     { 0.000000f,  0.000000f, -0.618034f},
     {-0.309017f, -0.500000f,  0.190983f}},
    {{-0.190983f,  0.309017f, -0.500000f},
     {-0.500000f, -0.190983f,  0.309017f},
     { 0.309017f, -0.500000f, -0.190983f}},
    {{-0.618034f,  0.000000f,  0.000000f},
     { 0.190983f, -0.309017f, -0.500000f},
     { 0.190983f, -0.309017f,  0.500000f}},
    {{-0.500000f,  0.190983f, -0.309017f},
     { 0.000000f, -0.618034f,  0.000000f},
     { 0.500000f,  0.190983f, -0.309017f}},
    {{-0.190983f, -0.309017f, -0.500000f},
     {-0.190983f, -0.309017f,  0.500000f},
     { 0.618034f,  0.000000f,  0.000000f}},
    {{-0.309017f, -0.500000f, -0.190983f},
     { 0.190983f,  0.309017f, -0.500000f},
     { 0.500000f, -0.190983f,  0.309017f}},
    {{ 0.309017f, -0.500000f,  0.190983f},
     { 0.000000f,  0.000000f, -0.618034f},
     { 0.309017f,  0.500000f,  0.190983f}},
    {{ 0.190983f, -0.309017f, -0.500000f},
     { 0.500000f,  0.190983f,  0.309017f},
     {-0.309017f,  0.500000f, -0.190983f}},
    {{ 0.500000f, -0.190983f, -0.309017f},
     { 0.000000f,  0.618034f,  0.000000f},
     {-0.500000f, -0.190983f, -0.309017f}},
    {{ 0.309017f,  0.500000f, -0.190983f},
     {-0.500000f,  0.190983f,  0.309017f},
     {-0.190983f, -0.309017f, -0.500000f}},
};

/**
 * The inverses of the change-of-basis matrices for the middle triangles.
 *
 * The i-th matrix is the inverse of the change-of-basis matrix from
 * natural basis to the basis formed by T_i's middle triangle's vectors.
 */
const matrix3f_t _mid_inverses[10] = {
    {{-0.000000f,  1.000000f, -0.618034f},
     { 0.000000f, -1.000000f, -0.618034f},
     {-0.618034f,  0.000000f,  1.000000f}},
    {{-1.000000f,  0.618034f, -0.000000f},
     {-0.000000f, -1.000000f,  0.618034f},
     { 0.618034f, -0.000000f, -1.000000f}},
    {{-0.618034f, -0.000000f, -1.000000f},
     { 1.000000f, -0.618034f, -0.000000f},
     {-0.618034f,  0.000000f,  1.000000f}},
    {{-1.000000f, -0.618034f, -0.000000f},
     { 1.000000f, -0.618034f,  0.000000f},
     {-0.000000f,  1.000000f, -0.618034f}},
    {{-1.000000f, -0.618034f,  0.000000f},
     { 0.618034f,  0.000000f,  1.000000f},
     { 0.618034f,  0.000000f, -1.000000f}},
    {{-0.618034f, -0.000000f, -1.000000f},
     { 1.000000f,  0.618034f, -0.000000f},
     { 0.000000f, -1.000000f,  0.618034f}},
    {{ 0.000000f, -1.000000f, -0.618034f},
     { 0.000000f,  1.000000f, -0.618034f},
     { 0.618034f, -0.000000f,  1.000000f}},
    {{ 1.000000f, -0.618034f, -0.000000f},
     { 0.000000f,  1.000000f,  0.618034f},
     {-0.618034f,  0.000000f, -1.000000f}},
    {{ 1.000000f,  0.618034f, -0.000000f},
     {-1.000000f,  0.618034f,  0.000000f},
     { 0.000000f, -1.000000f, -0.618034f}},
    {{-0.000000f,  1.000000f,  0.618034f},
     {-1.000000f, -0.618034f, -0.000000f},
     { 0.618034f,  0.000000f, -1.000000f}},
};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   v  
  * @param[in]   inclusive  
  * @param[out]  
  * @retval      
  * @note        
  */
int geogrid_section(const Vector3f_t *v, bool inclusive)
{
    int i = _triangle_index(v, inclusive);
    if (i < 0) {
        return -1;
    }

    int j = _subtriangle_index(i, v, inclusive);
    if (j < 0) {
        return -1;
    }

    return 4 * i + j;
}

static int _neighbor_umbrella_component(int idx, int comp_idx)
{
    if (idx < 3) {
        return _neighbor_umbrellas[idx].components[comp_idx];
    }
    return (_neighbor_umbrellas[idx % 3].components[comp_idx] + 10) % 20;
}

static int _from_neighbor_umbrella(int idx,
                                    const Vector3f_t *v,
                                    const Vector3f_t *u,
                                    bool inclusive)
{
    /* The following comparisons between the umbrella's first and second
     * vertices' coefficients work for this algorithm because all vertices'
     * vectors are of the same length. */

    if (math_flt_equal(u->x, u->y)) {
        /* If the coefficients of the first and second vertices are equal, then
         * v crosses the first component or the edge formed by the umbrella's
         * pivot and forth vertex. */
        int comp = _neighbor_umbrella_component(idx, 0);

        Vector3f_t w;
        matrix3f_mul_vec(&w, &_inverses[comp % 10], v);

        if (comp > 9) {
            w.x = -w.x;
            w.y = -w.y;
            w.z = -w.z;
        }
        float x0 = w.vec3[_neighbor_umbrellas[idx % 3].v0_c0];
        if (math_flt_zero(x0)) {
            if (!inclusive) {
                return -1;
            }
            return comp;
        } else if (x0 < 0) {
            if (!inclusive) {
                return -1;
            }
            return _neighbor_umbrella_component(idx, u->x < u->y ? 3 : 2);
        }

        return comp;
    }

    if (u->y > u->x) {
        /* If the coefficient of the second vertex is greater than the first
         * one's, then v crosses the first, second or third component. */
        int comp = _neighbor_umbrella_component(idx, 1);
        
        Vector3f_t w;
        matrix3f_mul_vec(&w, &_inverses[comp % 10], v);

        if (comp > 9) {
            w.x = -w.x;
            w.y = -w.y;
            w.z = -w.z;
        }
        float x1 = w.vec3[_neighbor_umbrellas[idx % 3].v1_c1];
        float x2 = w.vec3[_neighbor_umbrellas[idx % 3].v2_c1];

        if (math_flt_zero(x1)) {
            if (!inclusive) {
                return -1;
            }
            return _neighbor_umbrella_component(idx, x1 < 0 ? 2 : 1);
        } else if (x1 < 0) {
            return _neighbor_umbrella_component(idx, 2);
        }

        if (math_flt_zero(x2)) {
            if (!inclusive) {
                return -1;
            }
            return _neighbor_umbrella_component(idx, x2 > 0 ? 1 : 0);
        } else if (x2 < 0) {
            return _neighbor_umbrella_component(idx, 0);
        }

        return comp;
    } else {
        /* If the coefficient of the second vertex is lesser than the first
         * one's, then v crosses the first, fourth or fifth component. */
        int comp = _neighbor_umbrella_component(idx, 4);

        Vector3f_t w;
        matrix3f_mul_vec(&w, &_inverses[comp % 10], v);

        if (comp > 9) {
            w.x = -w.x;
            w.y = -w.y;
            w.z = -w.z;
        }
        float x4 = w.vec3[_neighbor_umbrellas[idx % 3].v4_c4];
        float x0 = w.vec3[_neighbor_umbrellas[idx % 3].v0_c4];

        if (math_flt_zero(x4)) {
            if (!inclusive) {
                return -1;
            }
            return _neighbor_umbrella_component(idx, x4 < 0 ? 0 : 4);
        } else if (x4 < 0) {
            return _neighbor_umbrella_component(idx, 0);
        }

        if (math_flt_zero(x0)) {
            if (!inclusive) {
                return -1;
            }
            return _neighbor_umbrella_component(idx, x0 > 0 ? 4 : 3);
        } else if (x0 < 0) {
            return _neighbor_umbrella_component(idx, 3);
        }

        return comp;
    }
}

static int _triangle_index(const Vector3f_t *v, bool inclusive)
{
    /* w holds the coordinates of v with respect to the basis comprised by the
     * vectors of T_i */
    Vector3f_t w;
    matrix3f_mul_vec(&w, &_inverses[0], v);

    int zero_count = 0;
    int balance = 0;
    int umbrella = -1;

    if (math_flt_zero(w.x)) {
        zero_count++;
    } else if (w.x > 0) {
        balance++;
    } else {
        balance--;
    }

    if (math_flt_zero(w.y)) {
        zero_count++;
    } else if (w.y > 0) {
        balance++;
    } else {
        balance--;
    }

    if (math_flt_zero(w.z)) {
        zero_count++;
    } else if (w.z > 0) {
        balance++;
    } else {
        balance--;
    }

    switch (balance) {
    case 3:
        /* All coefficients are positive, thus return the first triangle. */
        return 0;
    case -3:
        /* All coefficients are negative, which means that the coefficients for
         * -w are positive, thus return the first triangle's opposite. */
        return 10;
    case 2:
        /* Two coefficients are positive and one is zero, thus v crosses one of
         * the edges of the first triangle. */
        return inclusive ? 0 : -1;
    case -2:
        /* Analogous to the previous case, but for the opposite of the first
         * triangle. */
        return inclusive ? 10 : -1;
    case 1:
        /* There are two possible cases when balance is 1:
         *
         * 1) Two coefficients are zero, which means v crosses one of the
         * vertices of the first triangle.
         *
         * 2) Two coefficients are positive and one is negative. Let a and b be
         * vertices with positive coefficients and c the one with the negative
         * coefficient. That means that v crosses the triangle formed by a, b
         * and -c. The vector -c happens to be the 3-th vertex, with respect to
         * (a, b), of the first triangle's neighbor umbrella with respect to a
         * and b. Thus, v crosses one of the components of that umbrella. */
        if (zero_count == 2) {
            return inclusive ? 0 : -1;
        }

        if (!math_flt_zero(w.x) && w.x < 0) {
            umbrella = 1;
        } else if (!math_flt_zero(w.y) && w.y < 0) {
            umbrella = 2;
        } else {
            umbrella = 0;
        }

        break;
    case -1:
        /* Analogous to the previous case, but for the opposite of the first
         * triangle. */
        if (zero_count == 2) {
            return inclusive ? 10 : -1;
        }

        if (!math_flt_zero(w.x) && w.x > 0) {
            umbrella = 4;
        } else if (!math_flt_zero(w.y) && w.y > 0) {
            umbrella = 5;
        } else {
            umbrella = 3;
        }
        w.x = -w.x;
        w.y = -w.y;
        w.z = -w.z;

        break;
    case 0:
        /* There are two possible cases when balance is 1:
         *
         * 1) The vector v is the null vector, which doesn't cross any section.
         *
         * 2) One coefficient is zero, another is positive and yet another is
         * negative. Let a, b and c be the respective vertices for those
         * coefficients, then the statements in case (2) for when balance is 1
         * are also valid here.
         */
        if (zero_count == 3) {
            return -1;
        }

        if (!math_flt_zero(w.x) && w.x < 0) {
            umbrella = 1;
        } else if (!math_flt_zero(w.y) && w.y < 0) {
            umbrella = 2;
        } else {
            umbrella = 0;
        }

        break;
    }

    switch (umbrella % 3) {
    case 0:
        w.z = -w.z;
        break;
    case 1:
        //w = {w.y, w.z, -w.x};

        w.x = w.y;
        w.y = w.z;
        w.z = -w.x;
        break;
    case 2:
        //w = {w.z, w.x, -w.y};

        w.x = w.z;
        w.y = w.x;
        w.z = -w.y;
        break;
    }

    return _from_neighbor_umbrella(umbrella, v, &w, inclusive);
}

static int _subtriangle_index(const unsigned int triangle_index,
                                        const Vector3f_t *v,
                                        bool inclusive)
{
    /* w holds the coordinates of v with respect to the basis comprised by the
     * vectors of the middle triangle of T_i where i is triangle_index */
    Vector3f_t w;
    matrix3f_mul_vec(&w, &_mid_inverses[triangle_index % 10], v);

    if (triangle_index > 9) {
        w.x = -w.x;
        w.y = -w.y;
        w.z = -w.z;
    }

    if ((math_flt_zero(w.x) || math_flt_zero(w.y) || math_flt_zero(w.z)) && !inclusive) {
        return -1;
    }

    /* At this point, we know that v crosses the icosahedron triangle pointed
     * by triangle_index. Thus, we can geometrically see that if v doesn't
     * cross its middle triangle, then one of the coefficients will be negative
     * and the other ones positive. Let a and b be the non-negative
     * coefficients and c the negative one. In that case, v will cross the
     * triangle with vertices (a, b, -c). Since we know that v crosses the
     * icosahedron triangle and the only sub-triangle that contains the set of
     * points (seen as vectors) that cross the triangle (a, b, -c) is the
     * middle triangle's neighbor with respect to a and b, then that
     * sub-triangle is the one crossed by v. */
    if (!math_flt_zero(w.x) && w.x < 0) {
        return 3;
    }
    if (!math_flt_zero(w.y) && w.y < 0) {
        return 1;
    }
    if (!math_flt_zero(w.z) && w.z < 0) {
        return 2;
    }

    /* If x >= 0 and y >= 0 and z >= 0, then v crosses the middle triangle. */
    return 0;
}

/*------------------------------------test------------------------------------*/


